#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/u_int8_multi_array.hpp"
#include <opencv2/opencv.hpp>

#include <chrono>

#include "final_exam/param.h"

class CamNode : public rclcpp::Node {
public:
  CamNode(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "Camera Node '%s' has been started.",
                name.c_str());
    // 初始化视频捕获设备
    cap_.open("assets/final_exam.avi");
    if (!cap_.isOpened()) {
      RCLCPP_ERROR(this->get_logger(), "无法打开视频文件");
      return;
    }

    // 创建图片话题发布者（改用 UInt8MultiArray 处理二进制数据）
    image_publisher_ = this->create_publisher<std_msgs::msg::UInt8MultiArray>(
        "video_frames", 10);
    // 创建定时器，定时发布视频帧
    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(static_cast<int>(1000 / param::fps)),
        [&]() { timer_callback(); });
  }

private:
  // 视频图片话题发布者（使用 UInt8MultiArray 传输二进制数据）
  rclcpp::Publisher<std_msgs::msg::UInt8MultiArray>::SharedPtr image_publisher_;
  // 视频流
  cv::VideoCapture cap_;
  // 定时器，定时发布视频帧
  rclcpp::TimerBase::SharedPtr timer_;

  void timer_callback() {
    cv::Mat frame;
    // 读取视频帧
    if (!cap_.read(frame)) {
      RCLCPP_WARN(this->get_logger(), "视频播放结束或无法读取视频帧");
      cap_.set(cv::CAP_PROP_POS_FRAMES, 0); // 重置到第一帧
      return;
    }

    // 检查帧是否有效
    if (frame.empty()) {
      RCLCPP_WARN(this->get_logger(), "Read empty frame from video");
      return;
    }

    // 编码为 JPEG 格式（使用正确的参数格式）
    std::vector<uchar> buf;
    std::vector<int> encode_params({cv::IMWRITE_JPEG_QUALITY, 90});

    if (!cv::imencode(".jpg", frame, buf, encode_params)) {
      RCLCPP_WARN(this->get_logger(), "Failed to encode frame to JPEG");
      return;
    }

    // 检查编码结果
    if (buf.empty() || buf.size() < 100) {
      RCLCPP_WARN(this->get_logger(),
                  "Encoded buffer size is too small: %zu bytes", buf.size());
      return;
    }

    std::string encoded_image(buf.begin(), buf.end());
    // 创建 UInt8MultiArray 消息并填充数据
    auto message = std_msgs::msg::UInt8MultiArray();
    message.data.assign(buf.begin(), buf.end());

    // 发布编码后的图片数据
    image_publisher_->publish(message);
    RCLCPP_INFO(this->get_logger(), "发布了一帧视频，大小: %zu bytes",
                buf.size());
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<CamNode>("camera_node");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}